
#include <string.h>
#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/Marker.h>
#include <boost/thread.hpp>
//#include <dynamic_reconfigure/server.h>//动态调参
//#include <whu_controller/art_car_controllerConfig.h>//动态调参
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <math.h>
#include <std_srvs/Empty.h>


#define PI 3.14159265358979
#define speed_num 1     //速度指令下标
#define turn_num 0     //旋转指令下标
serial::Serial ser;

/*新增全道路判断型flag,改变是否到达目标点的检查方式*/

/********************/
/* CLASS DEFINITION */
/********************/

class MySerial
{
    private:
        
        
        serial::Serial *privateser;
        ros::Timer timer0;
        ros::NodeHandle nh;
        ros::Publisher send_pub, real_odom_pub;
       
    public:
        MySerial(serial::Serial* ser);
        void buffer_init();
        typedef union serialbuffer{
        uint8_t raw[12];
        uint32_t idat[3];
        float fdat[3];
        } serialBuffer;
        serialBuffer serialRxBuffer, serialTxBuffer;
        nav_msgs::Odometry real_odom;
        //ros::Time current_time;
        double servo_mid,speed_stop, real_vel;
        void whu_callBack(geometry_msgs::Twist msg);
        void sleep(const ros::TimerEvent&);
        double p1,p0;  //舵机打角和占空比线性系数
        void setRealVel(double vel);
    
};

class L1Controller
{
    public:
        L1Controller(MySerial*);
        ~L1Controller();
        void initMarker();
        bool isForwardWayPt(const geometry_msgs::Point& wayPt, const geometry_msgs::Pose& car_Pose);
        double WayPtAwayFromLfwDist(const geometry_msgs::Point& wayPt, const geometry_msgs::Point& car_pos);
        double getYawFromPose(const geometry_msgs::Pose& car_Pose);        
        //double getEta(const geometry_msgs::Pose& car_Pose);
        double getCar2GoalDist();
        double getL1Distance(const double& _Vcmd);
        double getSteeringAngle(double eta);
        double getGasInput(const double& speed,const double& current_v);
        double speedFitting(const double deg);
        uint8_t isInLimitZone();
        double get_odom_car2WayPtVec(const geometry_msgs::Pose& car_Pose);
        void buffer_init();
        


        
    private:
        ros::NodeHandle n_;
        ros::Subscriber odom_sub, path_sub, goal_sub,vel_sub, limit_zone_sub, via_sub,clear_sub;
        ros::Publisher pub_, marker_pub, angle_sub_;
        ros::Timer timer1, timer2;
        ros::Time start_time;
        tf::TransformListener tf_listener;

        visualization_msgs::Marker points, line_strip, goal_circle, limit_zone_circle, via_circle;
        geometry_msgs::Pose carPose,poseToAngle;
        geometry_msgs::PoseStamped odom_goal,pose_,global_goal;
        geometry_msgs::Twist cmd_vel;
        geometry_msgs::Point odom_goal_pos,carPose_pos,forwardPt,limit_zone[20];
        nav_msgs::Odometry odom;
        nav_msgs::Path map_path, odom_path, via_path_points;
        MySerial *myserial;
        



	    boost::recursive_mutex configuration_mutex_;

        double L, Lfw, Lrv, Vcmd,turnVcmd, lfw, lrv, steering, u, v,curLfw,strLfw,eta,min_turning_radius;
        double baseAngle, Angle_gain, goalRadius,angleCeil,strOrCurve,time_,timeVa,timeMax;
        double Angle_gain_left_s, Angle_gain_left_c, Angle_gain_right_s, Angle_gain_right_c;
        double Angle_gain_left_k, Angle_gain_left_b, Angle_gain_right_k, Angle_gain_right_b;
        double SPEED_CONTROL_P,SPEED_CONTROL_I,SPEED_CONTROL_D,SPEED_CONTROL_P_BK,SpeedControlOut;
        double speed_model_C,speed_model_Angle, speed_model_Vel, break_a, generalACC;
        double limit_zone_R, limit_zone_V;
        double deltaAngle,steeringAngle[80],lastEta,diffEta,pathIdFloor;
        double *map_pathPt;
        float speed_error,speed_error_t1,speed_error_all;
        int controller_freq,ESCLMAX,ESCHMAX,odomInc,lastPathflag;
        bool foundForwardPt, goal_received, goal_reached,stop,Lflag,isObst[80],startflag,pathflag; //deadzone_state;

       // dynamic_reconfigure::Server<whu_controller::art_car_controllerConfig> *dsrv_;
        int limit_zone_num,pathIdFlag,curveCount,straightCount;

        ros::ServiceClient clear_costmaps_client_;
        std_srvs::Empty srv;

        struct PathId
        {
            int start_;//弯道起始点
            int end_;//弯道终点
            int flag;//0--normal,1--右U,2--右90,3--左U,4--左90
        }pathid[5];

        boost::recursive_mutex DeadReckoning_mutex_;
         struct QNode    //定义队列结点的数据结构
        {
            QNode *next; //指针域,指向下一个结点
	        geometry_msgs::Vector3 data;    //数据域，存储队列信息
        };
        struct LinkQueue    //定义队列的数据结构
        {
            QNode *front;      //队首指针,指向QNode类型的指针
	        QNode *rear;       //队尾指针
            QNode *last;       //上次终止的位置
        };

        LinkQueue *Q;
        geometry_msgs::Vector3 storage;
        void InitQueue(LinkQueue &Q);     //构造一个空的队列,初始化
        bool IsEmpty(LinkQueue &Q);    //判断队列是否为空
        void EnQueue(LinkQueue &Q,const geometry_msgs::Vector3& e);     //从队列尾部插入元素
        void DeQueue(LinkQueue &Q);   //从队列首部删除一个结点
        void DestoryQueue(LinkQueue &Q);       //销毁一个队列
        void DeadReckoning(QNode *p,const double& tim_la);//航位推算

       // void pathProc(const int& flag); //根据不同的路径更改相应参数
        void pathidproce(double *angle_,int *flag);//根据路径上的角度，给不同的角度处树不同的flag
      

       


        void odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg);
        void pathCB(const nav_msgs::Path::ConstPtr& pathMsg);
        void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg);
        void velCB(const geometry_msgs::Twist::ConstPtr& velMsg);
        //void reconfigureCB(whu_controller::art_car_controllerConfig &config, uint32_t level);
        void goalReachingCB(const ros::TimerEvent&);
        void controlLoopCB(const ros::TimerEvent&);
        void limit_zone_CB(const geometry_msgs::PointStamped::ConstPtr& zoneMsg);
        void clear_CB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& clearMsg);
        //void via_pointsCB(const geometry_msgs::PoseStamped::ConstPtr& via_msg);
}; // end of class

MySerial::MySerial(serial::Serial* ser):real_vel(0)
{
    buffer_init();
    ros::NodeHandle pn("~");
    send_pub = nh.advertise<geometry_msgs::Twist>("pid_cmd", 1);
    //servo
    pn.param("servo_mid", servo_mid, 1500.0);
    pn.param("speed_stop", speed_stop, 0.0);
    /*pn.param("p4", p4, 0.0);
    pn.param("p3", p3, 0.0);
    pn.param("p2", p2, 0.0);*/

    pn.param("p1", p1, -26.65);

    p0=servo_mid-p1*90;
    //pn.param("p0", p0, 3314);
    serialTxBuffer.fdat[speed_num] = speed_stop;
    serialTxBuffer.idat[turn_num] =servo_mid;

    timer0=nh.createTimer(ros::Duration((1.0)/100), &MySerial::sleep, this);

    real_odom_pub = nh.advertise<nav_msgs::Odometry>("Encoder/odom", 5);   //发布底层里程计数据
    //real_odom_setting
    real_odom.header.frame_id = "odom";
    real_odom.child_frame_id="base_footprint";
    
    // real_odom.pose.pose.position.x=real_odom.pose.pose.position.y=real_odom.pose.pose.position.z=0;
    // real_odom.pose.pose.orientation.x=real_odom.pose.pose.orientation.y=real_odom.pose.pose.orientation.z=real_odom.twist.twist.linear.y=real_odom.twist.twist.linear.z=real_odom.twist.twist.angular.x=real_odom.twist.twist.angular.y=real_odom.twist.twist.angular.z=0;
    // real_odom.pose.pose.orientation.w=1;
    // for(int covnum=0;covnum<=35;covnum++)
    // real_odom.twist.covariance[0]=0.01;

    ROS_INFO("[param] servo_mid: %f", servo_mid);
    ROS_INFO("[param] speed_stop: %f", speed_stop);
    /*ROS_INFO("[param] p4: %f", p4);
    ROS_INFO("[param] p3: %f", p3);
    ROS_INFO("[param] p2: %f", p2);*/
    ROS_INFO("[param] p1: %f", p1);
    ROS_INFO("[param] p0: %f", p0);

    privateser = ser;
    
}
L1Controller::L1Controller(MySerial *useableSerial):L(0.35),Lrv(10.0),lfw(0.13),lrv(10.0),min_turning_radius(0.9),
                                                    strOrCurve(0.11),angleCeil(0.35),limit_zone_R(1.0),pathflag(false),
                                                    startflag(false),lastPathflag(0),lastEta(0.0),pathIdFlag(0),curveCount(0),
                                                    straightCount(0)
{
    //Private parameters handler
    ros::NodeHandle pn("~");

    //Car parameter
    pn.param("Vcmd", Vcmd, 3.0);
    pn.param("turnVcmd", turnVcmd, 3.0);
    pn.param("curLfw", curLfw, 0.6);
    pn.param("strLfw", strLfw, 0.9);
    pn.param("break_a",break_a,2.5);
    
    
    //Controller parameter
    pn.param("controller_freq", controller_freq, 100);
    //pn.param("AngleGain", Anglegain, -1.0);
    pn.param("Angle_gain_left_s", Angle_gain_left_s, -1.0);
    pn.param("Angle_gain_left_c", Angle_gain_left_c, -1.5);
    pn.param("Angle_gain_right_s", Angle_gain_right_s, -1.0);
    pn.param("Angle_gain_right_c", Angle_gain_right_c, -1.5);
    pn.param("baseAngle", baseAngle, 90.0);
    pn.param("timeVa", timeVa, 0.084);
    pn.param("diffEta", diffEta, 0.0);
    pn.param("pathIdFloor", pathIdFloor, 29.0);

    //ESC
    pn.param("SPEED_CONTROL_P", SPEED_CONTROL_P, 0.0);
    pn.param("SPEED_CONTROL_P_BK", SPEED_CONTROL_P_BK, 0.0);
    pn.param("SPEED_CONTROL_I", SPEED_CONTROL_I, 0.0);
    pn.param("SPEED_CONTROL_D", SPEED_CONTROL_D,0.0);

    pn.param("sm_Angle",speed_model_Angle, 15.0);     //速度模型V1：需要提供一对打角-速度对，以拟合标准曲线
    pn.param("sm_Vel",speed_model_Vel, 3.0);

    pn.param("limit_zone_V",limit_zone_V, 1.5);
    pn.param("generalACC",generalACC, 0.02);
    // generalACC
    //Publishers and Subscribers
    clear_costmaps_client_ = pn.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");

    odom_sub = n_.subscribe("/odometry/filtered", 1, &L1Controller::odomCB, this); //订阅里程计信息
    path_sub = n_.subscribe("/move_base_node/NavfnROS/plan", 1, &L1Controller::pathCB, this);//订阅路径信息
    goal_sub = n_.subscribe("/move_base_simple/goal", 1, &L1Controller::goalCB, this); //订阅目标点
    vel_sub = n_.subscribe("/cmd_vel", 1, &L1Controller::velCB, this);//订阅move_base的速度
    limit_zone_sub = n_.subscribe("/clicked_point", 1, &L1Controller::limit_zone_CB, this);//订阅临时limit_zone的位置
    //clear_sub = n_.subscribe("/initialpose", 1, &L1Controller::clear_CB, this);
    //via_sub = n_.subscribe("whu_via_point",1,&L1Controller::via_pointsCB,this);
    angle_sub_ = n_.advertise<geometry_msgs::PoseStamped>("angle", 1); 
    marker_pub = n_.advertise<visualization_msgs::Marker>("car_path", 10); // /car/path 队列长度10
    pub_ = n_.advertise<geometry_msgs::Twist>("car/cmd_vel", 1); //发布:   /car/cmd_vel 队列长度5
    

    //via_pub_ =n_.advertise<nav_msgs::Path>("/move_base/TebLocalPlannerROS/via_points",1); //发布teb必经点
    limit_zone_num = 0;
    deltaAngle = 0.0;
    //Timer
    timer1 = n_.createTimer(ros::Duration((1.0)/controller_freq), &L1Controller::controlLoopCB, this); // 频率 = controller_freq
    timer2 = n_.createTimer(ros::Duration((1.0)/controller_freq), &L1Controller::goalReachingCB, this); // 判断是否到达目标点 频率=2*controller_freq 
    //设置时间 controlLoopCB: 发布/cmd_vel  goalReachingCB:判断是否到达目标点,置位goal_reached(是否到达目标点) 和goal_received(是否收到目标点)两个标志位
    /*dsrv_ = new dynamic_reconfigure::Server<whu_controller::art_car_controllerConfig>(ros::NodeHandle("~"));
    dynamic_reconfigure::Server<whu_controller::art_car_controllerConfig>::CallbackType cb = boost::bind(&L1Controller::reconfigureCB, this, _1, _2);
    dsrv_->setCallback(cb);*/

    //Init variables
    goalRadius = getL1Distance(Vcmd);//Vcmd车速m/s 用来目标点半径L1
    foundForwardPt = false;
    goal_received = false; //目标点收到 标志位
    goal_reached = false; //目标点到达标志位
    Lflag=true;
    stop=false;//停车标志位
    cmd_vel.linear.x = 0.0; // 1500 for stop 
    cmd_vel.angular.z = baseAngle; //90
    //odomCount=0;
    start_time=ros::Time::now();
    ESCLMAX=1250;
    ESCHMAX=1700;
    speed_error_all = speed_error_t1 = speed_error = 0.0;
    //deadzone_state = true;

    // serialTxBuffer.fdat[speed_num] = speed_stop;
    // serialTxBuffer.idat[turn_num] =servo_mid;

    //cmd_last=deadSpeed;
    Angle_gain_left_k = (Angle_gain_left_s- Angle_gain_left_c) / (strLfw - curLfw);
    Angle_gain_left_b = Angle_gain_left_s - (strLfw * Angle_gain_left_k);
    Angle_gain_right_k = (Angle_gain_right_s - Angle_gain_right_c) / (strLfw - curLfw);
    Angle_gain_right_b = Angle_gain_right_s - (strLfw * Angle_gain_right_k);;

    //Show info
    ROS_INFO("[param] Vcmd: %f", Vcmd);
    ROS_INFO("[param] turnVcmd: %f", turnVcmd);
    //ROS_INFO("[param] Lfw: %f", Lfw);
    ROS_INFO("[param] strOrCurve(rad): %f", strOrCurve);
    ROS_INFO("[param] angleCeil(rad): %f", angleCeil);
    ROS_INFO("[param] curLfw: %f", curLfw);
    ROS_INFO("[param] strLfw: %f", strLfw);
    

    myserial = useableSerial;

    //计算速度模型常数
    speed_model_C = speed_model_Vel * sqrt(tan(speed_model_Angle*PI / 180.0));
    ROS_INFO("[param] sm_C: %f", speed_model_C);

    //Visualization Marker Settings
    initMarker();
    pose_.header.frame_id ="odom";

    

      //定义一个队列Q,存储下发时间,速度，角度信息
	Q = new LinkQueue;
    InitQueue(*Q); 
    //startflag=false;//直到下发速度不为0,更改为true
    //lastPathflag=0;//默认开始在直道上//normal
    time_=timeVa;//航位推算的起点
    timeMax=timeVa;//此时间前的队列均被删除
    ROS_INFO("timeVa:%.3f,timeMax:%.3f,time_: %f",timeVa,timeMax,time_);
    //pathcount=0;//visual
    ROS_WARN("hello_12");
    
}
L1Controller::~L1Controller()
{
    //delete dsrv_;
}
void L1Controller::InitQueue(LinkQueue &Q)     
{
	QNode *q;
	q = new QNode;    //申请一个结点的空间
	q->next = NULL;   //当作头结点
	//队首与队尾指针都指向这个结点，指针域为NULL
	Q.front = q;
	Q.rear = q;
    Q.last = q;
}

bool L1Controller::IsEmpty(LinkQueue &Q)    //判断队列是否为空
{
	if (Q.rear == Q.front)
		return true;
	else
		return false;
}

void L1Controller::EnQueue(LinkQueue &Q,const geometry_msgs::Vector3& e)     
{
	QNode *p;    //新创建一个结点
	p = new QNode;
	p->next = NULL;
	p->data = e;  //输入数据信息
   // (p->data).y = e.y;
    //(p->data).z = e.z;
	//将新结点插入队列尾部
	Q.rear->next = p;
	Q.rear = p;       //设置新的尾结点
}

void L1Controller::DeQueue(LinkQueue &Q)   
{
	QNode *p;
	p = Q.front->next;
	Q.front->next = p->next;       //将下一个结点当作头结点后面链接的第一个结点
	if (Q.rear == p)    //如果要删除的元素即为尾结点，则将头指针赋予尾指针，一同指向头结点，表示队列为空
		Q.rear = Q.front;
	delete p;
}

void L1Controller::DestoryQueue(LinkQueue &Q)     
{
	while (Q.front)
	{
		Q.rear = Q.front;    //从头节点开始，一个一个删除队列结点，释放空间
		delete Q.front;
		Q.front = Q.rear;
	}
}

void MySerial::buffer_init(){
  memset(serialTxBuffer.raw, 0xff, sizeof(serialTxBuffer));
  serialTxBuffer.fdat[0] = 0;

  
}

void MySerial::whu_callBack(geometry_msgs::Twist msg){   //键盘控制回调函数和controller

  serialTxBuffer.fdat[speed_num] = msg.linear.x;    //直接把car/cmd_vel 的线速度(pwm)发下去

  // if(msg->linear.x>1500)
  //   serialTxBuffer.fdat[speed_num] = speed_pos;
  // else if(msg->linear.x< 1500)
  //   serialTxBuffer.fdat[speed_num]=speed_nag;
  // else
  //   serialTxBuffer.fdat[speed_num]=speed_stop;  //正速度 负速度
  //ROS_WARN("p1:%lf\tp0:%lf",p1,p0);
  double angle=msg.angular.z;      //linear.z为角度形式,0-180,左180 中90 右0
  //下面选一个作为舵机转角和pwm的模型,一次到四次  一次p1 p2 p3 =0; 二次p1 p2=0;三次 p1 =0;
  //serialTxBuffer.idat[turn_num]=(int)(p4*angle*angle*angle*angle + p3*angle*angle*angle + p2*angle*angle + p1*angle + p0);
  int ms=p1*angle + p0;
  if(ms<500)
  ms=500;
  else if(ms>2500)
  ms=2500;
  serialTxBuffer.idat[turn_num]=ms;
  //current_time = ros::Time::now();
  ser.write(serialTxBuffer.raw, sizeof(serialTxBuffer));

  //ROS_INFO("Speed:%f\tturn:%d\tangle:%f\np1:%f\tp0:%f\t",serialTxBuffer.fdat[speed_num],serialTxBuffer.idat[turn_num],angle,p1,p0);
}
void L1Controller::initMarker()
{
    points.header.frame_id = line_strip.header.frame_id = goal_circle.header.frame_id = via_circle.header.frame_id="odom";
    limit_zone_circle.header.frame_id="map";
    points.ns = line_strip.ns = goal_circle.ns = limit_zone_circle.ns = via_circle.ns ="Markers";
    points.action = line_strip.action = goal_circle.action = limit_zone_circle.action = via_circle.action=visualization_msgs::Marker::ADD;
    points.pose.orientation.w = line_strip.pose.orientation.w = goal_circle.pose.orientation.w = limit_zone_circle.pose.orientation.w =via_circle.pose.orientation.w= 1.0;
    points.id = 0;
    line_strip.id = 1;
    goal_circle.id = 2;
    limit_zone_circle.id = 3;
    via_circle.id=14;
    points.type = visualization_msgs::Marker::POINTS;
    line_strip.type = visualization_msgs::Marker::LINE_STRIP;
    goal_circle.type = visualization_msgs::Marker::CYLINDER;
    limit_zone_circle.type = visualization_msgs::Marker::CYLINDER;
    via_circle.type=visualization_msgs::Marker::CYLINDER;
    // POINTS markers use x and y scale for width/height respectively
    points.scale.x = 0.2;
    points.scale.y = 0.2;

    //LINE_STRIP markers use only the x component of scale, for the line width
    line_strip.scale.x = 0.1;

    goal_circle.scale.x = 2*goalRadius;
    goal_circle.scale.y = 2*goalRadius;
    goal_circle.scale.z = 0.1;

    limit_zone_circle.scale.x = 2*limit_zone_R;
    limit_zone_circle.scale.y = 2*limit_zone_R;
    limit_zone_circle.scale.z = 0.1;

    via_circle.scale.x = 0.1;
    via_circle.scale.y = 0.1;
    via_circle.scale.z = 0.1;

    // Points are green
    points.color.g = 1.0;
    points.color.a = 1.0;

    // Line strip is blue
    line_strip.color.b = 1.0;
    line_strip.color.a = 1.0;

    //goal_circle is yellow
    goal_circle.color.r = 1.0;
    goal_circle.color.g = 1.0;
    goal_circle.color.b = 0.0;
    goal_circle.color.a = 0.5;

    //limit zone is red
    limit_zone_circle.color.r = 1.0;
    limit_zone_circle.color.g = 0.0;
    limit_zone_circle.color.b = 0.0;
    limit_zone_circle.color.a = 0.5;

    via_circle.color.r = 1.0;
    via_circle.color.g = 1.0;
    via_circle.color.b = 0.0;
    via_circle.color.a = 0.5;

 
}


void L1Controller::odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
    
    odom = *odomMsg;
    odomInc=0;
    boost::recursive_mutex::scoped_lock l(DeadReckoning_mutex_);
   // ROS_ERROR("odom");
    carPose = odom.pose.pose;
    //odomCount=0;
}


void L1Controller::pathCB(const nav_msgs::Path::ConstPtr& pathMsg)
{
    //ROS_ERROR("plan");
    //pathcount++;//visualization
    map_path = *pathMsg;
    map_pathPt=&(map_path.poses[0].pose.position.x);
    int a=map_path.poses.size();
    a= a> 82 ? 82 : a;
    double b[4];
    int flag=-1;
    memset(b, 0, 4*sizeof(double));//初始化
    memset(isObst, 1, 80*sizeof(bool));
    memset(steeringAngle, 0, 80*sizeof(double));
    memset(pathid, 0, 15*sizeof(int));
   // ROS_WARN("%d,%d,%d,%d,%d,%d,%d,%d,%d",pathid[0].start_,pathid[0].end_,pathid[0].flag,
    //pathid[1].start_,pathid[1].end_,pathid[1].flag,pathid[2].start_,pathid[2].end_,pathid[2].flag);
    for(int i=1;i<a-1;i++)
    {
        double a1=*(map_pathPt+13*i+13)-*(map_pathPt+13*i);
        double a2=*(map_pathPt+13*i-13)-*(map_pathPt+13*i);
        double b1=*(map_pathPt+13*i+14)-*(map_pathPt+13*i+1);
        double b2=*(map_pathPt+13*i-12)-*(map_pathPt+13*i+1);
        steeringAngle[i]=PI-acos((a1*a2+b1*b2)/sqrt(a1*a1+b1*b1)/sqrt(a2*a2+b2*b2));//以当前点为向量起始点，取前一点，后一点做终点，向量点积求夹角
        isObst[i]=(steeringAngle[i]<(strOrCurve));//小于6.5度，为直线上点
        if(steeringAngle[i]>0.52)//大于30度，路径规划出现蝴蝶结或麻花
        isObst[i]=1;//i点不合格，路径规划问题
        if((a1*b2-a2*b1)<0)//右转(对于顺时针，逆时针，向量叉乘，符号不同)
        steeringAngle[i]=-steeringAngle[i]/PI*180;
        else
        steeringAngle[i]=steeringAngle[i]/PI*180;
       // ROS_INFO("i:%d,isObst[i]:%d,steeringAngle[i]:%.3f",i,isObst[i],steeringAngle[i]);
    }
    //将连续弯道上的点转角(角度制)累加,根据范围竖不同flag,并记录起始点和终止点
    for(int i=1;i<a-1;i++)
    {
        if((i==1)&&(!isObst[1]))//一般第一点(从0 开始累计)为弯道上点，此处路径规划出现蝴蝶结，但我们又没滤掉
        continue;
        else if(flag<4)
        {
            if((!isObst[i])&&(isObst[i-1]))
            {
            if(flag!=-1)
            {
                pathidproce(&b[flag],&flag);
            }
            flag++;
            b[flag] +=steeringAngle[i];
            pathid[flag].start_=i;
            pathid[flag].end_=i;
            }
            else if((!isObst[i])&&(!isObst[i-1]))
            {
            b[flag] +=steeringAngle[i];
            if(isObst[i+1])
            pathid[flag].end_=i;
            }
        }
    } 
    if(flag!=-1)
    {
        pathidproce(&b[flag],&flag);
    }
    for(int i=0;i<4;i++)
    {
        if(pathid[i].flag!=0)
        {
            curveCount++;
            straightCount=0;
            if(curveCount==2)
            {
                curveCount=0;
                pathIdFlag=pathid[i].flag;
            }
            break;
            
        }
        if(i==3)
        {
            straightCount++;
            curveCount=0;
            if(straightCount==2)//防止误判
            {
                straightCount=0;
                pathIdFlag=0;
            }
        }
    }
   // ROS_ERROR("pathFlag_:%d",pathIdFlag);
    cmd_vel.angular.y=pathIdFlag;
    /*ROS_WARN("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,pathcount:%d,steeringAngle:%.3f,%.3f,%.3f",pathid[0].start_,pathid[0].end_,pathid[0].flag,
    pathid[1].start_,pathid[1].end_,pathid[1].flag,pathid[2].start_,pathid[2].end_,pathid[2].flag,pathid[3].start_,pathid[3].end_,pathid[3].flag,
    pathcount,b[0],b[1],b[2]);*/
}

void L1Controller::pathidproce(double *angle_,int *flag)
{
    if(fabs(*angle_)>fabs(pathIdFloor))
    {
        pathid[*flag].flag=1;//弯道
    }
    else 
    pathid[*flag].flag=0;//直道
   /* if(*angle_<-100)
    {
        pathid[*flag].flag=1;
    }  
    else if(*angle_<-pathIdFloor)
    {
        pathid[*flag].flag=2;
    } 
    else if(*angle_>100)
    {
        pathid[*flag].flag=3;
    }
        else if(*angle_>pathIdFloor)
    {
        pathid[*flag].flag=4;
    }*/
}

//接受到目标回调,goal_received = true;goal_reached = false;
void L1Controller::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg)
{
    global_goal=*goalMsg;//存储map下的目标
    try
    {
        tf_listener.transformPose("odom", ros::Time(0) , *goalMsg, "map" ,odom_goal);//目标初始在map下
        odom_goal_pos = odom_goal.pose.position;//x,y,z
        goal_received = true;
        goal_reached = false;// 收到消息置位标志位

        /*Draw Goal on RVIZ*/
        goal_circle.pose = odom_goal.pose;//position and orientation
        marker_pub.publish(goal_circle);
    }
    catch(tf::TransformException &ex)
    {
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
    }
    //via_pub_.publish(via_path_points);
    //ROS_ERROR("have published via_points");
   
}

//接收限速区的回调函数
void L1Controller::limit_zone_CB(const geometry_msgs::PointStamped::ConstPtr& zoneMsg)
{
    ROS_WARN("here");
    
    if(limit_zone_num >= 20) return;
    limit_zone[limit_zone_num] = zoneMsg->point;;//x,y,z
    limit_zone_num++;

    /*Draw Goal on RVIZ*/
    limit_zone_circle.pose.position = zoneMsg->point;//position and orientation
    marker_pub.publish(limit_zone_circle);
    limit_zone_circle.id++;
}

void L1Controller::clear_CB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& clearMsg)
{
    if(clear_costmaps_client_.call(srv))
    {
        ROS_WARN("succeed in clearing costmaps.");
    }
}

/*void L1Controller::via_pointsCB(const geometry_msgs::PoseStamped::ConstPtr& via_msg)
{
    ROS_WARN("there");
    geometry_msgs::PoseStamped via_p;
    try
    {
        tf_listener.transformPose("odom", ros::Time(0) , *via_msg, "map" ,via_p);//目标初始在map下

        //Draw Goal on RVIZ
        //via_path_points.poses.clear();
       via_path_points.header=via_p.header;
        via_path_points.poses.push_back(via_p);
        via_circle.id++;
        via_circle.pose.position=via_p.pose.position;
        marker_pub.publish(via_circle);
    }
    catch(tf::TransformException &ex)
    {
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
    }
    
    
}*/

void L1Controller::velCB(const geometry_msgs::Twist::ConstPtr& velMsg)
{
    if((*velMsg).linear.x==0)
    {
        stop=false;
    }
    else 
    {
        stop=false;
    }

}


//得到绕z轴的角度
double L1Controller::getYawFromPose(const geometry_msgs::Pose& car_Pose)
{
    float x = car_Pose.orientation.x;
    float y = car_Pose.orientation.y;
    float z = car_Pose.orientation.z;
    float w = car_Pose.orientation.w;

    double tmp,yaw;
    tf::Quaternion q(x,y,z,w);
    tf::Matrix3x3 quaternion(q);
    quaternion.getRPY(tmp,tmp, yaw);
    return yaw;
    
}
//判断规划点是否在车(odom)前面
bool L1Controller::isForwardWayPt(const geometry_msgs::Point& wayPt, const geometry_msgs::Pose& car_Pose)
{
    float car2wayPt_x = wayPt.x - car_Pose.position.x;
    float car2wayPt_y = wayPt.y - car_Pose.position.y;
    double car_theta = getYawFromPose(car_Pose);

    float car_car2wayPt_x = cos(car_theta)*car2wayPt_x + sin(car_theta)*car2wayPt_y;
    //float car_car2wayPt_y = -sin(car_theta)*car2wayPt_x + cos(car_theta)*car2wayPt_y;

    if(car_car2wayPt_x >0) /*is Forward WayPt*/
        return true;
    else
        return false;
}

//如果距离小于前瞻距离，返回false
double L1Controller::WayPtAwayFromLfwDist(const geometry_msgs::Point& wayPt, const geometry_msgs::Point& car_pos)
{
    double dx = wayPt.x - car_pos.x;
    double dy = wayPt.y - car_pos.y;
    double dist = sqrt(dx*dx + dy*dy);
    return dist;

    /*if(dist < Lfw)
        return false;
    else if(dist >= Lfw)
        return true;*/
}

/*void L1Controller::pathProc(const int& flag)
{
    if(flag==lastPathflag)
    return ;
    for(int c=0;c<5;c++)
    {
        if(flag==c)
        {
            Angle_gain_left_k = (Angle_gain_left_s[c] - Angle_gain_left_c[c]) / (strLfw - curLfw);
            Angle_gain_left_b = Angle_gain_left_s[c] - (strLfw * Angle_gain_left_k);
            Angle_gain_right_k = (Angle_gain_right_s[c] - Angle_gain_right_c[c]) / (strLfw - curLfw);
            Angle_gain_right_b = Angle_gain_right_s[c] - (strLfw * Angle_gain_right_k);
            if(c==0)
            pathflag=false;
            else
            pathflag=true;
            if(flag==2||flag==4)
            {
                if(time_!=timeVa[1])
                {
                    odomInc=0;
                    carPose = odom.pose.pose;
                }
                time_=timeVa[1];
            }
            else
            {
                if(time_!=timeVa[0])
                {
                    odomInc=0;
                    carPose = odom.pose.pose;
                }
                time_=timeVa[0];
            }
            break;
        }
        
    }
    lastPathflag=flag;
    ROS_ERROR("flag:%d",flag);
    return;
}*/
double L1Controller::get_odom_car2WayPtVec(const geometry_msgs::Pose& car_Pose)
{
    //ROS_WARN("lookforward");
     //%从odom得到
    carPose_pos = car_Pose.position;//x,y,z
    //double carPose_yaw;
    double carPose_yaw = getYawFromPose(car_Pose);//绕z轴的角度
    //geometry_msgs::Point forwardPt;
    geometry_msgs::Point odom_car2WayPtVec;
    foundForwardPt = false;
    //bool pointflag=true;
 //判断是否要前进,如果前进,将目标点放入forwardPt中,foundForwardPt = true
    
    if(!goal_reached){
        
        //ROS_ERROR("hello");
        /*double  a=0.0;
        if((*myserial).real_vel<0)
        a=0.0;
        else if((*myserial).real_vel<4)
        a=(*myserial).real_vel*time_;*/
        //double a=time_(*myserial).real_vel;
        //ROS_WARN("a:%.3f,(*myserial).real_vel:%.3f,time_:%.3f",a,(*myserial).real_vel,time_);
        //double a=3*time_;
        for(int i =0; i< map_path.poses.size(); i++)
        {
            //ROS_WARN("you are a fool.");
           // geometry_msgs::PoseStamped map_path_pose = map_path.poses[i];//规划路径的xyz和四元数
           // geometry_msgs::PoseStamped odom_path_pose;

            //try
            //{
                 //从路径规划得到odom_path_wayPt
                //const std::string& target_frame, const ros::Time& target_time,const geometry_msgs::PointStamped& pin,
                // const std::string& fixed_frame, geometry_msgs::PointStamped& pout
                //tf_listener.transformPose("odom", ros::Time(0) , map_path_pose, "map" ,odom_path_pose);
                geometry_msgs::Point odom_path_wayPt = map_path.poses[i].pose.position;
                bool _isForwardWayPt = isForwardWayPt(odom_path_wayPt,car_Pose);
               // ROS_WARN("_isForwardWayPt:%d,odom_path_wayPt(%.3f,%.3f,%.3f),car_Pose(%.3f,%.3f,%.3f),%.3f",_isForwardWayPt,odom_path_wayPt.x,odom_path_wayPt.y,odom_path_wayPt.z,car_Pose.position.x,car_Pose.position.y,car_Pose.position.z,carPose_yaw);

                if(_isForwardWayPt)
                {
                   // ROS_WARN("we in _isForwardWayPt");
                    double _WayPtAwayFromLfwDist = WayPtAwayFromLfwDist(odom_path_wayPt,carPose_pos);//规划路径的xyz和四元数从map下映射到odom下，存入odom_path_pose
                    //aim to advance control
                    //ROS_WARN("2");
                   /* if(a==0.0)
                    {
                        pointflag=false;
                    }
                    else if(pointflag&&a>0&&a<_WayPtAwayFromLfwDist)
                    {
                        //forwardPt = odom_path_wayPt; //目标点放在forwardPt中
                        carPose_pos=odom_path_wayPt;
                        carPose_yaw = getYawFromPose(map_path.poses[i].pose);
                        pointflag=false;
                        //ROS_WARN("pointflag:%d",i);
                        foundForwardPt = true; //前进
                        Lfw=_WayPtAwayFromLfwDist;
                        
                        continue;
                    }*/
                    if(curLfw<_WayPtAwayFromLfwDist)
                    {
                        // ROS_WARN("we in");
                         odom_car2WayPtVec.x = cos(carPose_yaw)*(odom_path_wayPt.x - carPose_pos.x) + sin(carPose_yaw)*(odom_path_wayPt.y - carPose_pos.y);
                         odom_car2WayPtVec.y = -sin(carPose_yaw)*(odom_path_wayPt.x - carPose_pos.x) + cos(carPose_yaw)*(odom_path_wayPt.y - carPose_pos.y);
                        if(fabs(atan2(odom_car2WayPtVec.y,odom_car2WayPtVec.x))>angleCeil)//情况1,转角过大
                        {
                        forwardPt = odom_path_wayPt; //目标点放在forwardPt中
                       // forwardPo=map_path.poses[i].pose;
                        foundForwardPt = true; //前进
                        Lfw=_WayPtAwayFromLfwDist;
                        //ROS_WARN("atan2,we get it,i:%d",i);
                        //ROS_WARN("foundForwardPt:%d",foundForwardPt);
                        break;
                        }
                        else if(i>1&&(isObst[i]==0||isObst[i-1]==0))//情况2,弯道上点
                        {
                        forwardPt = odom_path_wayPt; //目标点放在forwardPt中
                       // forwardPo=map_path.poses[i].pose;
                        foundForwardPt = true; //前进
                        Lfw=_WayPtAwayFromLfwDist;
                         //ROS_WARN("isObst,we get it");
                       // ROS_WARN("foundForwardPt:%d",foundForwardPt);
                        break;
                        }
                        else if(_WayPtAwayFromLfwDist>strLfw)//情况3,直道限值
                        {
                        forwardPt = odom_path_wayPt; //目标点放在forwardPt中
                       // forwardPo=map_path.poses[i].pose;
                        foundForwardPt = true; //前进
                        Lfw=_WayPtAwayFromLfwDist;
                       //ROS_WARN("strLfw,we get it,i:%d",i);
                       // ROS_WARN("foundForwardPt:%d",foundForwardPt);
                        break;
                        }
                    }
                }
           // }
          /* catch(tf::TransformException &ex)
            {
                ROS_ERROR("%s",ex.what());
                ros::Duration(1.0).sleep();
            }*/
        }
       if(!foundForwardPt&&(!map_path.poses.empty()))//未到终点，又没选到车前面点，直接拉到局部路径最后一点
        {
            bool a=map_path.poses.empty();
           // ROS_INFO("go,%d,foundForwardPt:%d",a,foundForwardPt);
            forwardPt = map_path.poses.back().pose.position;
            foundForwardPt=true;
            Lfw= WayPtAwayFromLfwDist(forwardPt,carPose_pos);
           // ROS_WARN("we get it but it is the last");
        }
        
    }
    else if(goal_reached)
    {
        forwardPt = odom_goal_pos;
       // forwardPo=odom_goal.pose;
        foundForwardPt = false;
        //ROS_INFO("goal REACHED!");
    }
    //以车为坐标原点,求目标点在车前面和左右边的距离,车前向为x正,车左为y正,指向x正角度为0,右转角度为负
    odom_car2WayPtVec.x = cos(carPose_yaw)*(forwardPt.x - carPose_pos.x) + sin(carPose_yaw)*(forwardPt.y - carPose_pos.y);
    odom_car2WayPtVec.y = -sin(carPose_yaw)*(forwardPt.x - carPose_pos.x) + cos(carPose_yaw)*(forwardPt.y - carPose_pos.y);
    double eta=atan2(odom_car2WayPtVec.y,odom_car2WayPtVec.x);
    //return odom_car2WayPtVec;
    /*double eta=getYawFromPose(forwardPo)-carPose_yaw;
    pose_.header.stamp = ros::Time::now();
    pose_.pose.position=carPose_pos;
    pose_.pose.position.z=0.0;
    pose_.pose.orientation=tf::createQuaternionMsgFromYaw(eta);
    angle_sub_.publish(pose_);*/

    return eta;
}

//车指向目标点的转角
/*double L1Controller::getEta(const geometry_msgs::Pose& car_Pose)
{
    geometry_msgs::Point odom_car2WayPtVec = get_odom_car2WayPtVec(car_Pose);

    double eta = atan2(odom_car2WayPtVec.y,odom_car2WayPtVec.x);
    return eta;
}*/


double L1Controller::getCar2GoalDist()
{
    try
    {
        tf_listener.transformPose("odom", ros::Time(0) , global_goal, "map" ,odom_goal);//目标初始在map下
        odom_goal_pos = odom_goal.pose.position;
    }
    catch(tf::TransformException &ex)
    {
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
    }
    geometry_msgs::Point car_pose = odom.pose.pose.position;
    double car2goal_x = odom_goal_pos.x - car_pose.x;
    double car2goal_y = odom_goal_pos.y - car_pose.y;

    double dist2goal = sqrt(car2goal_x*car2goal_x + car2goal_y*car2goal_y);

    return dist2goal;
}

uint8_t L1Controller::isInLimitZone()
{
    if(limit_zone_num==0)
    return 0;
    geometry_msgs::PointStamped car_pose_,car_pose;
    car_pose_.header=odom.header;
    car_pose_.point=odom.pose.pose.position;
    try
    {
        tf_listener.transformPoint("map", ros::Time(0) , car_pose_, "odom" ,car_pose);
    }
    catch(tf::TransformException &ex)
    {
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
    }
    uint8_t i;
    double car2zone_x, car2zone_y, dist2goal;

    for(i=0;i<limit_zone_num;i++){
        car2zone_x = limit_zone[i].x - car_pose.point.x;
        car2zone_y = limit_zone[i].y - car_pose.point.y;
        dist2goal = sqrt(car2zone_x*car2zone_x + car2zone_y*car2zone_y);
        if(dist2goal < limit_zone_R){
            ROS_WARN("ENTERED NO %d ZONE!",i);
            return 1;
        }
    }

    return 0;
}

double L1Controller::getL1Distance(const double& _Vcmd)
{
    //double L1 = 0;
    /*if(_Vcmd < 1.34)
        L1 = 3 / 3.0;
    else if(_Vcmd > 1.34 && _Vcmd < 5.36)
        L1 = _Vcmd*2.24 / 3.0;
    else
        L1 = 12 / 3.0;*/
    return 1.0;
}
//角度制
double L1Controller::getSteeringAngle(double eta)
{
    double steeringAnge = -atan2((L*sin(eta)),(Lfw/2+lfw*cos(eta)))*(180.0/PI);//Lfw目标容差,也就是前瞻距离
    //ROS_INFO("Steering Angle = %.2f", steeringAnge);
    return steeringAnge;
}

//检查是否到目标点，goal_reached 
void L1Controller::goalReachingCB(const ros::TimerEvent&)
{

    if(goal_received) //收到目标点之后判断是否到达到达 
    {
        double car2goal_dist = getCar2GoalDist();
        if(car2goal_dist < goalRadius)
        {
            goal_reached = true;  // 到达 goal_reached = true   goal_received = false
            goal_received = false; 
            ROS_INFO("Goal Reached !");
        }
    }
}

//闭环，speed期望速度
double L1Controller::getGasInput(const double& speed,const double& current_v)
{
    double fP, fI, fD, fsum;
    double error_max = 2000.0;
    speed_error_t1 = speed_error;
	speed_error = speed- current_v;   //本次偏差
	speed_error_all += speed_error;
    //以下是PID控制
    if(speed_error < 0.0){
        fP = speed_error*SPEED_CONTROL_P_BK;
    }else{
        fP = speed_error*SPEED_CONTROL_P;
    }
	fD = (speed_error - speed_error_t1) * SPEED_CONTROL_D;
	fI = speed_error_all*SPEED_CONTROL_I;	    //速度偏差的I计算
    fsum = fP + fI + fD;
    if(fsum>0.0){
        SpeedControlOut = 1550 + fsum;
    }else if(fsum < -5.0){
        SpeedControlOut = 1310 + 2*fsum;
    }else{
        SpeedControlOut = 1550 + 50*fsum;
    }


    /*if(fsum>0.0){
        SpeedControlOut = 1550 + fsum;
        deadzone_state = true;
    }else if(fsum < -50.0){
        SpeedControlOut = 1450 + fsum;
        deadzone_state = false;
    }else if(deadzone_state){
        SpeedControlOut = 1550 + fsum;
    }else{
        SpeedControlOut = 1450 + fsum;
    }*/
  

	if (error_max < speed_error_all)					//限幅并改变变量为最大值，否则会影响PID控制
	{
		speed_error_all = error_max;
                Lflag=0;

	}
	else if (speed_error_all < -error_max)
	{
		speed_error_all = -error_max;
                Lflag=0;

	}
    else Lflag=1;
    return SpeedControlOut;


}

double L1Controller::speedFitting(const double deg){
    double v_calc1, v_final;
    double carVel = (*myserial).real_vel;
    double stopDistance = carVel*carVel - turnVcmd*turnVcmd;
    double actual_deg = fabs(deg);
    static int speed_hold_count = 0;
    static double v_calc2 = 1.5;   //8.18-0.0
    static double v_calc3 = 1.5;   //8.18-0.0

    //Method 1 根据抓地力公式计算速度
    actual_deg = actual_deg > 27.0 ? 27.0 : actual_deg;
    stopDistance = stopDistance / break_a / 2;
    v_calc1 = speed_model_C * sqrt(1.0/tan(fabs(actual_deg) * PI / 180.0));
    //v_calc1 = Vcmd>v_calc?v_calc:Vcmd;
    //v_calc1 = turnVcmd < v_calc1 ? v_calc:turnVcmd;

    //Method 2 根据刹车加速度计算入弯速度
    if(Lfw > curLfw){
        if(stopDistance < Lfw){
            speed_hold_count++;
        }else{
            speed_hold_count--;
        }
    }else{
        speed_hold_count--;
    }

    speed_hold_count = speed_hold_count < 2 ? speed_hold_count : 2;     //8.18-2
    speed_hold_count = speed_hold_count > -1 ? speed_hold_count : -1;

    if(speed_hold_count < 0){
        v_calc2 = turnVcmd;
    }else{
        v_calc2 += generalACC;
        v_calc2 = v_calc2 < Vcmd ? v_calc2 : Vcmd;
    }

    //ROS_ERROR("calcDist:%f\tLfw:%f\tcalc2:%f",stopDistance,Lfw,v_calc2);

    if(pathIdFlag){
        v_calc3 = turnVcmd;
    }else{
        v_calc3 += generalACC;
        v_calc3 = v_calc3 < Vcmd ? v_calc3 : Vcmd;
    }

    //选取最低的速度作为目标速度
    v_final = v_calc2;
    //v_final = v_calc1 > v_calc3 ? v_calc3 : v_calc1;
    
    //最后判断是否为人为设定的限速区域
    if(isInLimitZone()) v_final = v_final < limit_zone_V ? v_final : limit_zone_V;
    
    v_final = v_calc1 > v_final ? v_final : v_calc1;

    v_calc2 = v_calc3 = v_final;

    return v_final;
}

void L1Controller::DeadReckoning(QNode *p,const double& tim_la)
{
        double s=((p->next->data).x-tim_la)*((p->data).y);
        if(s==0) //情况1,未前进
        return ;
        double carPose_yaw=getYawFromPose(carPose);
        if((p->data).z==0)   //情况2,无转角
        {
            carPose.position.x +=s*cos(carPose_yaw);
            carPose.position.y +=s*sin(carPose_yaw);
           // ROS_INFO("s:%.3f",s);

        }
        else    //情况3,简化为走弧线的航位推算
        {
            double radius = L/tan((p->data).z);
            if (fabs(radius) < min_turning_radius)
                radius=radius/(fabs(radius))*min_turning_radius;
            double deltaTheta=s/radius;
            double l=2*s*sin(deltaTheta/2)/deltaTheta;
            carPose.position.x += l*cos(carPose_yaw+deltaTheta/2);
            carPose.position.y += l*sin(carPose_yaw+deltaTheta/2);
            carPose_yaw += deltaTheta;
            carPose.orientation=tf::createQuaternionMsgFromYaw(carPose_yaw);
            //ROS_INFO("s:%.3f,radius:%.3f,deltaTheta:%.3f,l:%.3f,",s,radius,deltaTheta,l);
        }
       //ROS_WARN("carPose(%.3f,%.3f,%.3f)",carPose.position.x,carPose.position.y,carPose.position.z);
}
void L1Controller::controlLoopCB(const ros::TimerEvent&)
{
    double fittedSpeed = 0.0;
    geometry_msgs::Twist carVel = odom.twist.twist;
    cmd_vel.linear.x = 0.0;
    cmd_vel.angular.z = baseAngle;
    //ROS_WARN("1");

    if(goal_received)
    {
        //当队列不为空，对tim_st，tim_end中的点进行航位推算，并删除tim_purge之前的点，缓解舵机延迟
        //ROS_WARN("we are in DeadReckoning");
        if(!IsEmpty(*Q)&&(odomInc<5))//odomInc>4一般情况下，odom出现意外
       {
        bool flag=true;
        double tim_end=ros::Time::now().toSec();
        double tim_st=tim_end-time_;
        double tim_purge=tim_end-timeMax;
        double tim_la;
        QNode *p=Q->front->next;
       // ROS_WARN("tim_purge:%.3f,tim_st:%.3f,tim_end:%.3f",tim_purge,tim_st,tim_end);
        while(flag)
        {
            //ROS_INFO("(p->data).x:%.3f",(p->data).x);
            if((p->data).x<tim_purge)
            {
                DeQueue(*Q);
                p=Q->front->next;
                //ROS_INFO("(p->data).x:%.3f",(p->data).x);
            }
            else{
                //ROS_INFO("(p->data).x:%.3f",(p->data).x);
              if((p->data).x<tim_st)
              {
                //ROS_WARN("time:%.3f,v:%.3f",(p->data).x,(p->data).y)
                 p=p->next;
              }
              else
              {
                  //  ROS_INFO("hello2");
                  //ROS_INFO("Hello,odomInc:%d",odomInc);
                  if(odomInc>0)
                  { 
                      p=Q->last;
                  }
                  //ROS_INFO("tim_end:%.3f,tim_st:%.3f",tim_end,tim_st);
                  tim_la=(p->data).x;//用大于tim_st，小于tim_end的点与后面一点进行DeadReckoning
                  odomInc+=1;
                  boost::recursive_mutex::scoped_lock ecl(DeadReckoning_mutex_);
                  while((p->data).x<tim_end&&(Q->rear!=p))
                  {
                    //ROS_INFO("tim_la:%.3f",tim_la);
                    DeadReckoning(p,tim_la);
                    p=p->next;
                    tim_la=(p->data).x;
                  }
                  poseToAngle=carPose;
                  Q->last=p;
                  flag=false;
                }
             // ROS_INFO("hello3");

            }
            // ROS_INFO("out");
            if(IsEmpty(*Q))
            { 
                poseToAngle=carPose;
                flag=false;
            }
                 
        }
        //odomCount +=1;
        //ROS_WARN("%d",odomCount);
        }
        else
        {
            poseToAngle=carPose;
        }
        
        //double a=getYawFromPose(carPose);
                //ROS_ERROR("%.3f",a);
        //ROS_WARN("we are out");
        pose_.header.stamp = ros::Time::now();
        pose_.pose=poseToAngle;
        angle_sub_.publish(pose_);
        /*Estimate Steering Angle*/
        //ROS_WARN("3");
        eta = get_odom_car2WayPtVec(poseToAngle);  
        if(foundForwardPt)
        {
            double x = Lfw;
            x = x > strLfw ? strLfw : x;
            double Eta=getSteeringAngle(eta);
            if(Eta<0){
                Angle_gain = x * Angle_gain_left_k + Angle_gain_left_b;
            }else{
                Angle_gain = x * Angle_gain_right_k + Angle_gain_right_b;
            }
            
            deltaAngle = Eta*Angle_gain+diffEta*(Eta-lastEta);
            lastEta=Eta;
            cmd_vel.angular.z = baseAngle + deltaAngle;//得出角度制的角度，前方90度
            /*Estimate Gas Input*/
            if(!goal_reached)
            {
                //double u = getGasInput(carVel.linear.x);
                //cmd_vel.linear.x = baseSpeed - u;//小于预期速度,减小x
                
                if(stop)
                {
                    cmd_vel.linear.x = 0.0;
                }
                else
                {
                        fittedSpeed = speedFitting(deltaAngle);
                        cmd_vel.linear.x = fittedSpeed;
                        //ROS_INFO("Turn:%f\tThrottle:%f",cmd_vel.angular.z,cmd_vel.linear.x);
                }
                
                /*if(cmd_vel.linear.x-cmd_last>0.23)
                {
                    cmd_vel.linear.x=cmd_last+0.23;
                }
                else if(cmd_last-cmd_vel.linear.x>1.2)
                {
                    cmd_vel.linear.x=cmd_last-1.2;
                }*/

               // cmd_last=cmd_vel.linear.x;
                
               // ROS_INFO("\nGas = %.2f\nSteering angle = %.2f\nspeed=%.2f\n",cmd_vel.linear.x,cmd_vel.angular.z,carVel.linear.x);
            }
        }
    }

    (*myserial).whu_callBack(cmd_vel);
    start_time=ros::Time::now();
    if((!startflag)&&((*myserial).real_vel)>0&&(cmd_vel.linear.x>0))//不想记录一些无用的数据，不想增加计算量
    {
        startflag=true;
    }
    else if(startflag)
    {
        storage.x=ros::Time::now().toSec();
        storage.y=cmd_vel.linear.x;
       // ROS_INFO("cmd_vel.linear.x:%.3f",cmd_vel.linear.x);
        storage.z=deltaAngle;
        //ROS_INFO("storage.x:%.3f,cmd_vel.linear.x:%.3f,deltaAngle:%.3f",storage.x,cmd_vel.linear.x,deltaAngle);
        EnQueue(*Q,storage);
    }
    //ROS_WARN("cmd_vel.angular.z:%.3f",cmd_vel.angular.z);
     /*Visualized Target Point on RVIZ*/
    /*Clear former target point Marker*/
    points.points.clear();
    line_strip.points.clear();
    
    if(foundForwardPt && !goal_reached)//车和路径规划的point填到末尾
    {
        points.points.push_back(carPose_pos);
        points.points.push_back(forwardPt);
        line_strip.points.push_back(carPose_pos);
        line_strip.points.push_back(forwardPt);
        marker_pub.publish(points);
        marker_pub.publish(line_strip);
       // ROS_INFO("havd ");
    }

    
    cmd_vel.linear.y = fittedSpeed;
    cmd_vel.linear.z=speed_error_all;
    cmd_vel.angular.x=(double)((*myserial).serialTxBuffer.idat[turn_num]);
    pub_.publish(cmd_vel);
}

/*void L1Controller::reconfigureCB(whu_controller::art_car_controllerConfig &config, uint32_t level)
{
   boost::recursive_mutex::scoped_lock l(configuration_mutex_);
   
}*/

void MySerial::setRealVel(double vel){
    real_vel = vel;
}

void MySerial::sleep(const ros::TimerEvent&)  //串口接收并发布topic
{
    // if(ros::Time::now() - current_time > ros::Duration(1)){
    //   serialTxBuffer.fdat[speed_num]=speed_stop;
    //   serialTxBuffer.idat[turn_num]=servo_mid;
    //   ser.write(serialTxBuffer.raw, sizeof(serialTxBuffer));
    // }
    geometry_msgs::Twist cmd;

    if(ser.available()){
     //ros::Time current_time = ros::Time::now();
       while(ser.available() < 12);
       ser.read(serialRxBuffer.raw , 12);
       ser.flushInput();
       cmd.linear.x=serialRxBuffer.fdat[0];
       cmd.linear.y=serialRxBuffer.fdat[1];
       cmd.linear.z=serialRxBuffer.fdat[2];
       send_pub.publish(cmd);
       real_odom.header.stamp=ros::Time::now();
       real_odom.twist.twist.linear.x = cmd.linear.y;
       real_odom_pub.publish(real_odom);
       setRealVel(cmd.linear.y);
       //ROS_WARN("target:%f\t%f\t%f\t",serialRxBuffer.fdat[0],serialRxBuffer.fdat[1],serialRxBuffer.fdat[2]);
     }
}


/*****************/
/* MAIN FUNCTION */
/*****************/
int main(int argc, char **argv)
{

    
    //Initiate ROS
    ros::init(argc, argv, "art_car_controller_v10_9");
    
    try{
        ser.setPort("/dev/car");
        ser.setBaudrate(115200);
    
        serial::Timeout to = serial::Timeout::simpleTimeout(10);
        ser.setTimeout(to);
        ser.open();
    }
    catch(serial::IOException& e){
        ROS_ERROR_STREAM("Unable to open port");
        return -1;
    }
  
        if(ser.isOpen()){
        ROS_INFO_STREAM("Serial Port Initialized");
        ser.setDTR(false);
        ser.setRTS(false);
    }else{
        return -1;
    }
    
    MySerial myserial(&ser);
    L1Controller controller(&myserial);//初始化包含3个消息回调，2个定时中断，1个server回调


    ros::spin();
    return 0;
}
